#ifndef __UDSF_PROTOCOL_C_
#define __UDSF_PROTOCOL_C_

#include "udf_global.h"
/*
*   notice:
*   UDSF 同UDF编程：不可调用cstd-lib,只可调用"sdk_ifs_udk_cfg.h"、"sys_api.h"中列出的API函数。
*   UDSF文件内函数均需使用static修饰符，且UDF中以#include "UDSF.c"形式引用
*/

static int udsf_pointrst_ProtoGene(uint8_t* buff,DBSCANOUT* dbscanclu,STRUCT_POINTRST *frameRst,uint16_t delta,uint8_t numplvs,uint8_t proto_ext_flag,uint8_t proto_draw_style) __attribute__((unused));

static int udsf_pointrst_ProtoGene(uint8_t* buff,DBSCANOUT* dbscanclu,STRUCT_POINTRST *frameRst,uint16_t delta,uint8_t numplvs,uint8_t proto_ext_flag,uint8_t proto_draw_style){
	int index = 0;
	int len = 0;
	
	if( proto_ext_flag ){ // 协议扩展v11
		len += 10;
	}
	if( numplvs & 0x1 ){
		len += frameRst->targetNum * 8 + 3;
	}
	if( numplvs > 1 ){
		len += dbscanclu->numCluster * 12 + 3;
	}
	
	len += 3;
	
	buff[index++] = 0x55;
	buff[index++] = 0xAA;
	
	buff[index++] = len;
	buff[index++] = len>>8;
	buff[index++] = len>>16;
	buff[index++] = len>>24;
	
	buff[index++] = delta;
	buff[index++] = delta>>8;
	buff[index++] = numplvs;
	if( numplvs & 0x1){
		buff[index++] = 0x01;
		buff[index++] = frameRst->targetNum;
		buff[index++] = frameRst->targetNum>>8;
		
		for( int i = 0;i< frameRst->targetNum;i++ ){
			for( int j=0;j<8;j++ ){
				buff[index++] = ((uint8_t*)(&frameRst->target[i]))[j];
			}
		}
	}
	if( numplvs > 1 ){
		buff[index++] = 0x02;
		buff[index++] = dbscanclu->numCluster;
		buff[index++] = dbscanclu->numCluster>>8;

		for( int i = 0;i< dbscanclu->numCluster;i++ ){
			for( int j=0;j<12;j++ ){
				buff[index++] = ((uint8_t*)(&dbscanclu->cluster[i]))[j];
			}
		}
	}
	if( proto_ext_flag ){
		buff[index++] = 0xAB;
		buff[index++] = 0xCD;
		buff[index++] = 0x10;// v1.0 扩展协议
		buff[index++] = proto_draw_style; // bit7 abs扩展，bit6 cache，bit0~bit5： 0-小点 1-中点 2-大点
		#ifdef RANGE_REFINEMENT
		buff[index++] = UDFDEF_RANGE_USE*UDFDEF_RANGE_INTER_FACE;// 最大距离点数
		#else
		buff[index++] = UDFDEF_RANGE_USE;
		#endif
		buff[index++] = UDFDEF_CHIRP_NUM;
		buff[index++] = ((RANGE_RES_MM)&0x00ff);
		buff[index++] = ((RANGE_RES_MM)&0xff00)>>8;
		buff[index++] = (VEL_RES_MM_S&0xff);
		buff[index++] = (VEL_RES_MM_S>>8)&0xff;
	}
	return index;
}

#include "udsf_cstdlike.c"
static void udsf_protov11_add_epc_data(uint8_t *buff,int *p_offset,struct epc *p_epc,uint16_t* p_target_cnt) __attribute__((unused));
static void udsf_protov11_frame_end(uint8_t* buff,int *p_offset,uint16_t target_cnt) __attribute__((unused));
static void udsf_protov11_frame_start(uint8_t* buff,int *p_offset,uint16_t delta,uint8_t draw_style,uint8_t rangeMax,uint8_t chirpMax,uint16_t range_res_mm,uint16_t vel_res_mm_s) __attribute__((unused));

static void udsf_protov11_add_epc_data(uint8_t *buff,int *p_offset,struct epc *p_epc,uint16_t* p_target_cnt){
	
	int index = *p_offset;
	struct epc_target  * p_epc_target = p_epc->target;
	
	for( int i = 0;i< p_epc->target_cnt;i++ ){
		
		buff[index++] = p_epc_target[i].info1;
		buff[index++] = p_epc_target[i].info2;
		int data_type = (p_epc->target[i].info1>>3)&0x7;
		if( data_type == 0x1 ){
			udsf_memcpy((uint8_t*)&p_epc_target[i].rIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].vIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].sinPhiIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].pow,&buff[index],4);
			index += 4;
		}else if ( data_type == 0x2 ){
			udsf_memcpy((uint8_t*)&p_epc_target[i].rIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].vIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].sinPhiIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].pow,&buff[index],4);
			index += 4;
		}else{
			uint16_t u16_data = p_epc_target[i].rIdx>>12;
			udsf_memcpy((uint8_t*)&u16_data,&buff[index],2);
			index += 2;
			
			int8_t s8_data = p_epc_target[i].vIdx>>12;
			udsf_memcpy((uint8_t*)&s8_data,&buff[index],1);
			index += 1;
			
			s8_data = p_epc_target[i].sinPhiIdx>>12;
			udsf_memcpy((uint8_t*)&s8_data,&buff[index],1);
			index += 1;
			
			udsf_memcpy((uint8_t*)&p_epc_target[i].pow,&buff[index],4);
			index += 4;
		}
	}

	*p_target_cnt += p_epc->target_cnt;
	*p_offset = index;
}

static void udsf_protov11_frame_end(uint8_t* buff,int *p_offset,uint16_t target_cnt){
	int len = *p_offset - 6;
	
	buff[2] = len;
	buff[3] = len>>8;
	buff[4] = len>>16;
	buff[5] = len>>24;
	
	buff[22] = target_cnt; 
	buff[23] = target_cnt>>8;
}

static void udsf_protov11_frame_start(uint8_t* buff,int *p_offset,uint16_t delta,uint8_t draw_style,uint8_t rangeMax,uint8_t chirpMax,uint16_t range_res_mm,uint16_t vel_res_mm_s){
	int index = *p_offset;

	
	buff[index++] = 0x55;
	buff[index++] = 0xAA;
	
	/* len */
	buff[index++] = 0x0; 
	buff[index++] = 0x0;
	buff[index++] = 0x0;
	buff[index++] = 0x0;
	
	buff[index++] = delta;
	buff[index++] = delta>>8;
	buff[index++] = 0x01;
	buff[index++] = 0x01;
	buff[index++] = 0x00;
	buff[index++] = 0x00;

	buff[index++] = 0xAB;
	buff[index++] = 0xCD;
	buff[index++] = 0x11;// v1.0 扩展协议
	buff[index++] = draw_style;

	buff[index++] = rangeMax;
	buff[index++] = chirpMax;
	buff[index++] = range_res_mm;
	buff[index++] = range_res_mm>>8;
	buff[index++] = vel_res_mm_s;
	buff[index++] = vel_res_mm_s>>8;
	
	/*target cnt*/
	buff[index++] = 0x0;
	buff[index++] = 0x0;
	
	*p_offset = index;
}
#endif


